package com.hitqz.robot.dispatch.netty.strategy.publicPathStrategy.util;

import cn.hutool.core.collection.CollectionUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.alibaba.nacos.shaded.com.google.common.collect.Maps;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.google.common.cache.Cache;
import com.google.common.collect.HashBasedTable;
import com.google.common.collect.Table;
import com.hitqz.robot.api.business.dto.CmdRobot2PosDto;
import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
import com.hitqz.robot.api.business.entity.RobotEntity;
import com.hitqz.robot.api.business.entity.RobotGroupEntity;
import com.hitqz.robot.api.business.entity.RobotMapPosEntity;
import com.hitqz.robot.api.business.enums.WorkStatusEnum;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.biz.service.RobotService;
import com.hitqz.robot.dispatch.netty.util.JtsUtil;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.*;

import java.util.*;
import java.util.concurrent.*;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.stream.Collectors;

/**
 * @author xupkun
 * @date 2025/1/3
 */
@Slf4j
public class DynamicCalculationUtil {

    private static final ThreadPoolExecutor threadPoolExecutor= new ThreadPoolExecutor(
            200,
            200,
            60,
            TimeUnit.SECONDS,
            new LinkedBlockingQueue<Runnable>(200),
            Executors.defaultThreadFactory(),
            new ThreadPoolExecutor.DiscardOldestPolicy());

    private static final String lockA = "path_list_lock";

    private static final String lockB = "path_list_action_lock";

    private static final String lockC = "move_other_path_lock";

    //private static String avoidPoint = "AP37";

    /**
     * key 机器人编码
     * value 目标位置
     */
    private static final Map<String, RobotMapPosDto> ROBOT_TARGET_POS = Maps.newHashMap();
    /**
     * 更换机器人目标位置--中间过程点/ 记录的是如果前往中间点，一开始提交的最终目标点
     * key 机器人编码
     * value 目标位置
     */
    private static final Map<String, RobotMapPosDto> ROBOT_FINAL_POS = Maps.newHashMap();

    /**
     * key clientID
     * value 中间点code
     */
    private static final Map<String, String> ROBOT_MIDDLE_POS = Maps.newHashMap();

    /**
     * key 机器人编码
     * boolean true 暂停导航
     * 是否暂停
     */
    private static final Map<String, Boolean> ROBOT_STOP_FLAG = Maps.newHashMap();

    /**
     * 是否已经发送过点位
     */
    private static final Map<String, Boolean> ROBOT_SEND_POS_FLAG = Maps.newHashMap();

    /**
     * key 机器人编码
     * value 路径列表
     * 机器人路径列表
     */
    private static final Map<String, LinkedList<RobotMapPosDto>> ROBOT_PATH_LIST = Maps.newHashMap();

    /**
     * 公共路径数据
     */
    private static final Map<String, Boolean> ROBOT_BLOCK_ROBOT = Maps.newHashMap();

    /**
     *  避让 主机器人-》 避让的机器人-〉 避让的点位
     */
    public static Table<String, String, RobotMapPosDto> ROBOT_LOCK_TARGET_POS = HashBasedTable.create();





    private static final Table<String, String, List<RobotMapPosDto >> ROBOT_COMMON_PATH = HashBasedTable.create();


    private static final Map<String, Boolean> AVOID_ROBOT = Maps.newConcurrentMap();

    /**
     * 计算策略---动态切换路线
     * @param groupId
     */
    public static void dispatchMonitor(Integer groupId, List<RobotEntity> robotEntityList) {
        robotEntityList.forEach(t-> {
            if (CollectionUtil.isNotEmpty(ROBOT_PATH_LIST.get(t.getRobotSn()))) {
                // 判断导航状态
                boolean hasBlock = checkOtherCarBlock(groupId, t.getRobotSn(), null);
                if (hasBlock) {
                    // 暂停导航
                    if(!ROBOT_STOP_FLAG.get(t.getRobotSn()) || ROBOT_STOP_FLAG.get(t.getRobotSn()) == null) {
                        log.info("{} dispatchMonitor:暂停导航: {}-- {}", t.getRobotSn(), !ROBOT_STOP_FLAG.get(t.getRobotSn()), ROBOT_STOP_FLAG.get(t.getRobotSn()) == null);
                        ROBOT_STOP_FLAG.put(t.getRobotSn(), true);
                        changeNavStatus(t.getRobotSn(), 1);
                    }
                } else {
                    if (ROBOT_STOP_FLAG.get(t.getRobotSn())) {
                        ROBOT_STOP_FLAG.put(t.getRobotSn(), false);
                        changeNavStatus(t.getRobotSn(), 2);
                        log.info("下发继续执行任务" + t.getRobotSn());
                    }
                }
            }
        });
   }

    /**
     * 提交路径
     * @param group
     * @param robotApplyPathDto
     */
    public static void submitPath(RobotGroupEntity group, RobotApplyPathDto robotApplyPathDto) {
        // 提交路径
        if (robotApplyPathDto.getPathList().size() == 0) {
            //log.info("{} 提交的没有路径,清除数据 ",  robotApplyPathDto.getClientId());
            clearRobot(robotApplyPathDto.getClientId());
            return;
        }
        clearRobot(robotApplyPathDto.getClientId());
        ROBOT_PATH_LIST.remove(robotApplyPathDto.getClientId());
        LinkedList<RobotMapPosDto> pathList = robotApplyPathDto.getPathList();
        String robotSn = robotApplyPathDto.getClientId();
        ROBOT_SEND_POS_FLAG.put(robotSn, false);
        // 最后要前往的终点，可能存在中间点，但是不影响最后的目标点
        ROBOT_FINAL_POS.put(robotSn, robotApplyPathDto.getPathList().get(robotApplyPathDto.getPathList().size() -1));
        // 计算能不能运动
        boolean hasBlock = checkOtherCarBlock(group.getId(), robotSn, robotApplyPathDto.getPathList());
        while (hasBlock) {
            //log.info("{} 机器人被其他车占用,不下发点位", robotApplyPathDto.getClientId());
            hasBlock = checkOtherCarBlock(group.getId(), robotSn, robotApplyPathDto.getPathList());
        }
        log.info("{} 机器人可以正常执行任务 {}", robotApplyPathDto.getClientId(), hasBlock);
        // 设置路径

        //ROBOT_TARGET_POS.put(robotSn, pathList.get(pathList.size()-1));
        setTargetPos(robotSn, pathList.get(pathList.size()-1));
        log.info("锁定目标成功: {}-- {}", robotApplyPathDto.getClientId(), pathList.get(pathList.size()-1).getCode());
        DynamicCalculationUtil.getOrSetPathList(robotApplyPathDto.getClientId(), robotApplyPathDto.getPathList(), true);
        //可以前往目标点后，锁定目标点，然后进行计算
        ROBOT_SEND_POS_FLAG.put(robotSn, true);
        // 继续执行任务，提示可以前往目标点
        goFinalPos(robotSn, robotApplyPathDto.getPathList().get(robotApplyPathDto.getPathList().size()-1));
    }


    public static void setTargetPos(String robotSn, RobotMapPosDto robotMapPosDto) {
        synchronized (robotSn.intern()) {
            log.info("{} 设置目标点 {}", robotSn, robotMapPosDto.getCode());
            ROBOT_TARGET_POS.put(robotSn, robotMapPosDto);
        }
    }

    /**
     * 目标点是否占用/路径上是否被其他车堵住
     * @param mainClientId
     * @return
     */
    public static boolean checkOtherCarBlock(Integer groupId, String mainClientId,  LinkedList<RobotMapPosDto> mainClientPath) {
        //boolean isBlock = false;
        String blockRobot = "";
        AtomicBoolean isBlock = new AtomicBoolean(false);
        if (mainClientPath == null) {
            mainClientPath = ROBOT_PATH_LIST.get(mainClientId);
        }
        Cache<Integer, List<RobotEntity>> posInfoDtoCache = SpringUtil.getBean("robotDispatch");
        if (posInfoDtoCache.getIfPresent(groupId) != null) {
            List<RobotEntity> robotEntities = posInfoDtoCache.getIfPresent(groupId);
            assert robotEntities != null;
            Iterator<RobotEntity> iterator = robotEntities.iterator();
            RobotMapPosDto currentTargetPos = mainClientPath.get(mainClientPath.size() -1);
            Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
            while (iterator.hasNext()) {
                // 路径路径上上面是否存在车进行阻挡
                RobotEntity otherRobot = iterator.next();
                RobotClientSummaryDto robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(otherRobot.getRobotSn());
                try {
                    assert robotClientSummaryDto != null;
                    if (Objects.requireNonNull(robotClientSummaryDto).getLogicOnlineStatus() == 0) {
                        // 调度下线的车不用循环管
                        continue;
                    }
                    if (!otherRobot.getRobotSn().trim().equalsIgnoreCase(mainClientId.trim())) {
                        // 计算目标点是否已经被其他车锁定
                        try {
                            RobotMapPosDto robotMapPosDto = ROBOT_TARGET_POS.get(otherRobot.getRobotSn());
                            if (robotMapPosDto != null && currentTargetPos != null &&
                                    currentTargetPos.getCode().equals(robotMapPosDto.getCode())) {
                                blockRobot = otherRobot.getRobotSn();
                                //log.info("{} 目标点被 {} 机器人 {} 占用", robotMapPosDto.getCode(), mainClientId, otherRobot.getRobotSn());
                                isBlock.set(true);
                                break;
                            }
                        } catch (Exception e) {
                            log.info("{} 进行计算目标点是否被占用发生异常 {}", otherRobot.getRobotSn(), e);
                        }

                        // 计算其他的机器人的位置，是否有阻挡在路径上面
                        try {
                            if (CollectionUtil.isNotEmpty(mainClientPath)) {
                                for (int i = 0; i < mainClientPath.size() - 1; i++) {
                                    RobotMapPosDto start = mainClientPath.get(i);
                                    RobotMapPosDto end = mainClientPath.get(i + 1);
                                    Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
                                    Polygon polygon = robotPosPolygon.getIfPresent(otherRobot.getRobotSn());
                                    LineString lineSegment = JtsUtil.createLineString(start.getPosX(), start.getPosY(),
                                            end.getPosX(), end.getPosY());
                                    if (lineSegment.intersects(polygon)) {
                                        blockRobot = otherRobot.getRobotSn();
                                        //log.info("{} 有车在路上挡住需要停止： {} --- {}---{}", mainClientId, blockRobot, start.getCode(), end.getCode());
                                        isBlock.set(true);
                                        break;
                                    }
                                }
                            }
                        } catch (Exception e) {
                            log.info("{} 进行计算其他车是否被占用发生异常 {}", otherRobot.getRobotSn(), e);
                        }
                        try {
                            // 计算是不是存在公共路径
                            // 可能是任务中，判断公共路径状态，然后判断公共路径以及方向，同向的话-如果距离太近的话，需要暂停
                            if(!isBlock.get()) {
                                LinkedList<RobotMapPosDto> blockRobotPath = ROBOT_PATH_LIST.get(otherRobot.getRobotSn());
                                // 计算重叠的路径 2. 计算方向 3. 计算位置相对距离， <5 主动停止
                                if (CollectionUtil.isNotEmpty(blockRobotPath) && blockRobotPath.size() > 1){
                                    //log.info("{}、{} 的路径是 {}", mainClientId, otherRobot.getRobotSn(),
                                    //        blockRobotPath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
                                    List<LineString> overlappingSegments = JtsUtil.findOverlappingSegments(mainClientPath, blockRobotPath);
                                    List<RobotMapPosDto> samePath = JtsUtil.convertOverlappingSegmentsToPath(blockRobotPath, overlappingSegments);
                                    String direction = JtsUtil.calculatePathDirection(mainClientPath, blockRobotPath);
                                    //log.info("{}、{}运动的路径方向是 {}, 公共路径为: {}", mainClientId, otherRobot.getRobotSn(),
                                    //        direction, samePath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
                                    ROBOT_COMMON_PATH.put(mainClientId, otherRobot.getRobotSn(), samePath);
                                    if (CollectionUtil.isNotEmpty(samePath) && samePath.size() > 1) {
                                        if (direction.equals("同向")) {
                                            // 同向的话计算谁在前，谁在后， 如果距离过近，则暂停后面的车， 不然的话，同向是可以允许运行
                                            robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(mainClientId);
                                            RobotClientSummaryDto blockClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(otherRobot.getRobotSn());
                                            assert robotClientSummaryDto != null;
                                            assert blockClientSummaryDto != null;
                                            int result = JtsUtil.calculateRelativePositionOnSamePath(
                                                    new LinkedList<>(samePath), robotClientSummaryDto.getPosition().getLaserPos().getX(),
                                                    robotClientSummaryDto.getPosition().getLaserPos().getY(),
                                                    blockClientSummaryDto.getPosition().getLaserPos().getX(),
                                                    blockClientSummaryDto.getPosition().getLaserPos().getY()
                                            );
                                            // 通行的车前面，阻塞的车在  只要知道后面的车就行。
                                            // 计算距离
                                            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
                                            double distance = Objects.requireNonNull(robotPosPolygon.getIfPresent(mainClientId)).
                                                    distance(robotPosPolygon.getIfPresent(otherRobot.getRobotSn()));
                                            // 后面的车与前面的车距离 7米，考虑暂停
                                            //log.info("{}、{} 公共路径长度为 {}---{}", mainClientId, otherRobot.getRobotSn(), result, distance);
                                            if (result == -1 ) {
                                                if (distance < 7) {
                                                    // 计算相对距离，如果距离在于一定的距离内，则暂停
                                                    //log.info("{}、{} 路径方向相同距离过近 {} ，暂停", mainClientId, otherRobot.getRobotSn(), distance);
                                                    //isBlock.set(true);
                                                    break;
                                                } else {

                                                }
                                            } else {
                                                //isBlock.set(true);
                                            }
                                        } else {
                                            // 反向或者是其他的情况
                                            RobotMapPosDto finalCode = ROBOT_TARGET_POS.get(blockRobot);
                                            if (finalCode == null) {
                                                log.info("{} 路径为{}、{} 路径为{} 运动的路径方向是 {}, 公共路径为: {}", mainClientId,
                                                        mainClientPath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")),
                                                        otherRobot.getRobotSn(), blockRobotPath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")),
                                                        direction, samePath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
                                                isBlock.set(true);
                                                break;
                                            }
                                        }
                                    }
                                }
                            }
                        } catch (Exception e) {
                            log.info("计算距离发生异常:" + e);
                        }
                        try {
                            // 计算是不是存在公共路径 -- 计算相较的车的距离。距离小于7的话就要停止
                            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
                            //double distance = Objects.requireNonNull(robotPosPolygon.getIfPresent(mainClientId)).
                            //        distance(robotPosPolygon.getIfPresent(otherRobot.getRobotSn()));
                        } catch (Exception e) {
                            log.info("计算距离发生异常:" + e);
                        }
                    }
                } catch (Exception e) {
                    log.info("判断其他车的状态异常：{}", e);
                }
            }
        }
        // 查看路径上面是不是有可能存在路径上的点被其他车占用
        // 判断是否静止，驱动堵塞的车先去避让
        if (isBlock.get()) {
            String finalBlockRobot = blockRobot;
            LinkedList<RobotMapPosDto> finalMainClientPath = mainClientPath;
            String finalBlockRobot1 = blockRobot;
            threadPoolExecutor.submit(()-> {
                synchronized (lockC.intern()) {
                    if (StrUtil.isEmpty(ROBOT_MIDDLE_POS.get(finalBlockRobot))) {
                        Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
                        RobotClientSummaryDto robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(finalBlockRobot);
                        if (robotClientSummaryDto != null) {
                            if (robotClientSummaryDto.getLogicOnlineStatus() == 0) {
                                // 有一台是逻辑(调度)下线的，存在不可预知的问题，进行报警，通知人工进行处理
                                //log.info("阻挡的车是调度下线,处于不可控制状态");
                            } else {
                                // 如果是逻辑(调度)上线，只要是非任务状态就控制驱赶去其他位置，表示可以控制， 然后控制驱动
                                // if (Double.parseDouble(robotClientSummaryDto.getStatus().getSpeed()) == 0) {
                                // 空闲/任务中，等待路径释放
                                // 目标点位为空，代表的是可能只是放置
                                RobotMapPosDto blockRobotTargetPos = ROBOT_TARGET_POS.get(finalBlockRobot);
                                if ((robotClientSummaryDto.getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.FREE.getCode()))
                                    || (Double.parseDouble(robotClientSummaryDto.getStatus().getSpeed()) == 0
                                        && robotClientSummaryDto.getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.WORK.getCode())
                                )
                                ) {
                                    // 阻挡的AGV/机器人处于空闲中，直接前往其他空闲的点进行等待
                                    //log.info("速度为0驱动其他车前往其他点位置，查询最近的处于路径之外的点{} ", finalBlockRobot);
                                    // 1. 获取全部的点
                                    // 2. 获取从缓存中获取车当前的位置
                                    // 3. 过滤得到路径外面的点
                                    // 4. 计算当前点到路径的距离，然后取最近的点，然后进行导航
                                    String code = findNearestPointOutsidePath(mainClientId, finalBlockRobot, finalMainClientPath);
                                    if(StrUtil.isNotBlank(code) && (AVOID_ROBOT.get(finalBlockRobot) == null || !AVOID_ROBOT.get(finalBlockRobot))) {
                                        log.info("{} 驱动AGV前往 {}---执行的AGV是{}----目标点{} ",mainClientId, code, finalBlockRobot, blockRobotTargetPos);
                                        AVOID_ROBOT.put(finalBlockRobot, true);
                                        ROBOT_MIDDLE_POS.put(finalBlockRobot, code);
                                        isBlock.set(true);
                                    }
                                }  else {
                                    // 可能是任务中，判断公共路径状态，然后判断公共路径以及方向，同向的话-如果距离太近的话，需要暂停
                                    LinkedList<RobotMapPosDto> blockRobotPath = ROBOT_PATH_LIST.get(finalBlockRobot1);
                                    // 计算重叠的路径 2. 计算方向 3. 计算位置相对距离， <5 主动停止
                                    List<LineString> overlappingSegments = JtsUtil.findOverlappingSegments(finalMainClientPath, blockRobotPath);
                                    List<RobotMapPosDto> samePath = JtsUtil.convertOverlappingSegmentsToPath(blockRobotPath, overlappingSegments);
                                    String direction = JtsUtil.calculatePathDirection(finalMainClientPath, blockRobotPath);
                                    if (CollectionUtil.isNotEmpty(samePath) && direction.equals("同向")) {
                                         // 同向的话计算谁在前，谁在后， 如果距离过近，则暂停后面的车， 不然的话，同向是可以允许运行
                                        robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(finalBlockRobot);
                                        RobotClientSummaryDto blockClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(finalBlockRobot);
                                        assert robotClientSummaryDto != null;
                                        assert blockClientSummaryDto != null;
                                        int result = JtsUtil.calculateRelativePositionOnSamePath(
                                                 new LinkedList<>(samePath), robotClientSummaryDto.getPosition().getLaserPos().getX(),
                                                 robotClientSummaryDto.getPosition().getLaserPos().getY(),
                                                 blockClientSummaryDto.getPosition().getLaserPos().getX(),
                                                 blockClientSummaryDto.getPosition().getLaserPos().getY()
                                        );
                                        // 通行的车前面，阻塞的车在  只要知道后面的车就行。
                                        // 计算距离
                                        //Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
                                        //double distance = Objects.requireNonNull(robotPosPolygon.getIfPresent(mainClientId)).
                                        //        distance(robotPosPolygon.getIfPresent(finalBlockRobot2));
                                        //// 后面的车与前面的车距离 7米，考虑暂停
                                        //if (result == -1 && distance < 7) {
                                        //    // 计算相对距离，如果距离在于一定的距离内，则暂停
                                        //    //log.info("{}、{} 路径方向相同距离过近 {} ，暂停", mainClientId, finalBlockRobot, distance);
                                        //    isBlock.set(true);
                                        //} else {
                                        //    //log.info("{}、{} 路径方向相同距离大于一定距离 {}，认为不是阻挡", mainClientId, finalBlockRobot, distance);
                                        //    isBlock.set(false);
                                        //}
                                    }
                                }
                            }
                        }
                    } else {
                        // 不为空的时候，计算公共路径数据

                    }
                }
            });
        }

        ROBOT_BLOCK_ROBOT.put(mainClientId, isBlock.get());
        return isBlock.get();
    }


    public static void getOrSetPathList(String robotSn, LinkedList<RobotMapPosDto> robotPathList,Boolean setFlag) {
        synchronized (lockA) {
            if (setFlag) {
                log.info("{} 提交的机器人路径:{}", robotSn, robotPathList.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
                ROBOT_PATH_LIST.put(robotSn, robotPathList);
            } else {
                List<RobotMapPosDto> currentPathList = ROBOT_PATH_LIST.get(robotSn);
                if (currentPathList == null || currentPathList.isEmpty()) {
                    return;
                }
                // 到达点位
                Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
                if (posInfoDtoCache == null) {
                    return;
                }
                try {
                    RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
                    if (robotClientSummaryDto == null) {
                        return;
                    }
                    String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
                    String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getLastPosCode();
                    if (StrUtil.isEmpty(nearStation) && StrUtil.isEmpty(lastArrivePos)) {
                        return;
                    }

                    int pathListSize = currentPathList.size();
                    int arrivePosIndex = -1;
                    for (int i = 0; i < pathListSize; i++) {
                        if (lastArrivePos.equalsIgnoreCase(currentPathList.get(i).getCode())) {
                            arrivePosIndex = i;
                            break;
                        }
                    }

                    if (arrivePosIndex == -1) {
                        return;
                    }
                    // 增加同步控制
                    synchronized (lockB) {
                        currentPathList.remove(arrivePosIndex);
                        ROBOT_PATH_LIST.put(robotSn, new LinkedList<>(currentPathList));
                        if (currentPathList.isEmpty()) {
                            //log.info("已经执行完路径，清空所有数据" + robotSn);
                            RobotMapPosDto finalCode = ROBOT_FINAL_POS.get(robotSn);
                            String middlePos = ROBOT_MIDDLE_POS.get(robotSn);
                            if (StrUtil.isNotEmpty(middlePos)) {
                                //log.info("可能是驱赶其他车辆，然后前往中间点，继续计算能不能前往目标点{}---{}", finalCode.getCode(), middlePos);
                                //ROBOT_MIDDLE_POS.remove(robotSn);
                            } else {
                                //log.info("可能是任务执行完成了{}---{}", finalCode.getCode(), middlePos);
                                ROBOT_PATH_LIST.put(robotSn, new LinkedList<>());
                            }
                            clearRobot(robotSn);
                            //log.info("清空后的路径数据"+ ROBOT_PATH_LIST.get(robotSn).size());
                        }
                    }

                } catch (Exception e) {
                    //log.error("Error processing robot path: {}", e.getMessage(), e);
                }
            }
        }
    }


    /**
     * 清空数据
     * @param robotSn
     */
    public static void clearRobot(String robotSn) {
        ROBOT_TARGET_POS.put(robotSn, null);
        ROBOT_FINAL_POS.put(robotSn, null);
        ROBOT_PATH_LIST.put(robotSn, new LinkedList<>());
    }


    /**
     * 修改中间点
     * @param clientId
     */
    public static void changeMiddlePos(Integer groupId, String mainClientId, String clientId, String code, RobotMapPosDto finalCode) {
        // 更换目标
        CmdRobot2PosDto dto = new CmdRobot2PosDto();
        dto.setClientId(clientId);
        dto.setPosCode(code);
        //dto.setPosCode(avoidPoint);
        SpringUtil.getApplicationContext().publishEvent(dto);
        log.info("{} 下发中间的点之后，在前往目标点 {}", clientId, code);
        CompletableFuture.runAsync(()-> {
            while (true) {
                try {
                    Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
                    RobotClientSummaryDto blockRobotSummaryDto = robotClientSummaryDtoCache.getIfPresent(clientId);
                    if (blockRobotSummaryDto.getPosition().getLaserPos().getLastPosCode().equals(code)) {
                        Thread.sleep(5000);
                        RobotMapPosDto goTargetPos = ROBOT_TARGET_POS.get(clientId);
                        if (goTargetPos != null) {
                            ROBOT_MIDDLE_POS.remove(goTargetPos);
                            while (!checkOtherCarBlock(groupId, clientId, null)
                                    && (ROBOT_BLOCK_ROBOT.get(mainClientId) != null && !(ROBOT_BLOCK_ROBOT.get(mainClientId)))) {
                                log.info("中间点的过程中无阻挡，可以继续往后面走了 {}--{}", clientId, goTargetPos.getCode());
                                AVOID_ROBOT.put(clientId, false);
                                goFinalPos(clientId, goTargetPos);
                                break;
                            }
                            break;
                        } else {
                            log.info("中间点已经到达，但是目标点已经不存在了，直接结束 {}--- {}", clientId, code);
                            break;
                        }
                    }
                } catch (Exception e) {

                }
            }
        });
    }


    /**
     * 修改中间点
     * @param clientId
     */
    public static void goFinalPos(String clientId, RobotMapPosDto finalPos) {
        // 更换目标
        CmdRobot2PosDto dto = new CmdRobot2PosDto();
        dto.setClientId(clientId);
        dto.setPosCode(finalPos.getCode());
        SpringUtil.getApplicationContext().publishEvent(dto);
        log.info("{} 下发可以前往目标站点:{}",clientId, finalPos.getCode());
    }


    /**
     * 修改导航状态
     * @param clientId
     * @param status
     */
    public static void changeNavStatus(String clientId, int status) {
        SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(clientId, status));
    }



    /**
     * 查询其他车路径外距离车位置最近的点
     * @param mainClientId
     * @return 最近的路径外点
     */
    public static String findNearestPointOutsidePath(String mainClientId, String blockRobotId,LinkedList<RobotMapPosDto> mainPath ) {
        String code = "";
        // 判断路径是否为空，或者
        if (CollectionUtil.isEmpty(mainPath)) {
            return code;
        }
        // 获取当前车的位置
        Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
        Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
        RobotClientSummaryDto mainRobotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(mainClientId);
        if (mainRobotClientSummaryDto == null) {
            return null;
        }
        RobotMapPosService robotMapPosService = SpringUtil.getBean(RobotMapPosService.class);
        RobotService robotService = SpringUtil.getBean(RobotService.class);
        RobotEntity robotEntity = robotService.selectByRobotSn(mainClientId);

        List<RobotMapPosEntity> robotMapPosDtoList = robotMapPosService.list(Wrappers.<RobotMapPosEntity>lambdaQuery()
                            .eq(RobotMapPosEntity::getMapCode, robotEntity.getMapCode()));
        if (CollectionUtil.isNotEmpty(robotMapPosDtoList)) {
            List<RobotMapPosEntity> pointsOutsidePath = new ArrayList<>();
            for (RobotMapPosEntity point : robotMapPosDtoList) {
                if (!isPointOnPath(point, mainPath)) {
                    pointsOutsidePath.add(point);
                }
            }
            if (CollectionUtil.isNotEmpty(pointsOutsidePath)) {
                double minDistance = Double.MAX_VALUE;
                for (RobotMapPosEntity point : pointsOutsidePath) {
                    // 位置可能会发生改变，比如遥控器控制
                    RobotClientSummaryDto blockRobotSummaryDto = robotClientSummaryDtoCache.getIfPresent(blockRobotId);
                    Polygon mainCar = robotPosPolygon.getIfPresent(mainClientId);
                    assert blockRobotSummaryDto != null;
                    double distance = calculateDistance(blockRobotSummaryDto.getPosition().getLaserPos().getX(),
                            blockRobotSummaryDto.getPosition().getLaserPos().getY()
                            , point.getPosX(), point.getPosY());
                    if (distance < minDistance) {
                        // 判断是不是与主车有相交，如果有则当作废弃
                        Point gemPoint = JtsUtil.getPoint(point.getPosX(), point.getPosY());
                        assert mainCar != null;
                        if (!mainCar.intersects(gemPoint)) {
                            RobotMapPosDto posCode = ROBOT_TARGET_POS.get(mainClientId);
                            if (posCode != null &&  point.getCode().equals(posCode.getCode())) {
                                minDistance = distance;
                                code = point.getCode();
                            }
                            if (posCode == null) {
                                minDistance = distance;
                                code = point.getCode();
                            }
                        }

                    }
                }
            }
        }
       return code;
    }



    /**
     * 判断点是否在路径上
     * @param point 待判断的点
     * @param path 路径
     * @return 如果点在路径上返回true，否则返回false
     */
    private static boolean isPointOnPath(RobotMapPosEntity point, LinkedList<RobotMapPosDto> path) {
        if (CollectionUtil.isEmpty(path)) {
            return false;
        }
        GeometryFactory geometryFactory = new GeometryFactory();
        Coordinate pointCoord = new Coordinate(point.getPosX(), point.getPosY());
        LineString pointLine = geometryFactory.createLineString(new Coordinate[]{pointCoord, pointCoord});
        // 判断路径上每个线段与待判断点是否相交
        for (int i = 0; i < path.size() - 1; i++) {
            try {
                RobotMapPosDto start = path.get(i);
                RobotMapPosDto end = path.get(i + 1);
                LineString pathSegment = JtsUtil.createLineString(start.getPosX(), start.getPosY(), end.getPosX(), end.getPosY());
                if (pathSegment.intersects(pointLine)) {
                    return true;
                }
            } catch (Exception e) {
                log.info("计算是否在路径在上面:" + e.getMessage());
                return false;
            }

        }
        return false;
    }

    /**
     * 计算两点之间的距离
     * @param x1 第一个点的X坐标
     * @param y1 第一个点的Y坐标
     * @param x2 第二个点的X坐标
     * @param y2 第二个点的Y坐标
     * @return 两点之间的距离
     */
    private static double calculateDistance(double x1, double y1, double x2, double y2) {
        return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2));
    }





}


